int find_costbufferentry(uint8_t query_id, ADDRESS node_id)
{
  int i;

  for (i = 0; i < COSTBUFFER_SIZE; i++)
    if (query_buffer[query_id].groupopt.cost_buffer[i].used &&
      query_buffer[query_id].groupopt.cost_buffer[i].node_id == node_id)
      return i;

  return -1;
}

int find_freecostbufferentry(uint8_t query_id)
{
  int i;

  for (i = 0; i < COSTBUFFER_SIZE; i++)
    if (query_buffer[query_id].groupopt.cost_buffer[i].used == 0)
      return i;

  return -1;
}

uint8_t update_costbuffer(uint8_t query_id, ADDRESS node_id, ADDRESS source_id, int cost_diff,
  uint8_t cost_seq)
{
  int entry_index = find_costbufferentry(query_id, node_id);
  COSTBUFFER_ENTRY *p;

  if (entry_index < 0)
    entry_index = find_freecostbufferentry(query_id);
  if (entry_index < 0)
  {
    DBGERR("Cannot find free slot!");
    pause_sim();
    return 0;
  }

  p = &query_buffer[query_id].groupopt.cost_buffer[entry_index];
  if (p->used == 0 || (p->node_id == node_id && p->cost.plancost_seq < cost_seq))
  {
    p->used = 1;
    p->node_id = node_id;
    p->source_id = source_id;
    p->cost.plancost_seq = cost_seq;
    p->cost.plancost_diff = cost_diff;
    query_buffer[query_id].groupopt.cost_received = 1;

    return 1;
  }

  return 0;
}

void compute_sourcepath(ADDRESS *path, uint8_t *path_len, int source_index)
{
  int jointarget_index;

  if (querysource_buffer[source_index].path_len > 0)
  {
    *path_len = querysource_buffer[source_index].path_len;
    memcpy(path, querysource_buffer[source_index].path, *path_len*sizeof(ADDRESS));
  } else {
    jointarget_index = querysource_buffer[source_index].jointarget_index;

    *path_len = queryjointarget_buffer[jointarget_index].path_nodecount;
    memcpy(path, queryjointarget_buffer[jointarget_index].path_nodes, *path_len*sizeof(ADDRESS));
    if (querysource_buffer[source_index].id != queryjointarget_buffer[jointarget_index].id)
      path[(*path_len)++] = querysource_buffer[source_index].id;   // target node is join node
  }
}

void group_opt(uint8_t query_id, int source_index)
{
  int i, cost_diff;
  uint32_t old_cost, curr_cost;
  uint8_t curr_costupd, curr_coordupd, groupopt_type;
  ADDRESS source_node;
  QUERY_ENTRY *p;

DO_STACKTEST

  p = &query_buffer[query_id];
  groupopt_type = p->spec.params[PARAM_GROUPOPT];
  if (groupopt_type == GROUPOPT_NONE || p->options & (QUERYOPT_SENDTOROOT | QUERYOPT_PRUNE) ||
    p->spec.srcrel_count == 1)
    return;

  DBGOUT("Initiating group optimization");
  old_cost = 0;
  for (i = 0; i < SRCREL_COUNT; i++)
    old_cost += p->rel[i].multicast.innet_cost;
  optimize_plan();

  cost_diff = 0; curr_cost = 0;
  for (i = 0; i < SRCREL_COUNT; i++)
  {
    cost_diff += (int) p->rel[i].multicast.innet_cost - p->rel[i].multicast.base_cost;
    curr_cost += (int) p->rel[i].multicast.innet_cost;
  }
  curr_costupd = (curr_cost != old_cost);
  if (curr_costupd)
  {
    DBGOUT("Cost diff updated to %d, sequence: %d (%d, %d)", cost_diff, p->innet_seq, curr_cost, old_cost);
    update_costbuffer(query_id, TOS_NODE_ID, TOS_NODE_ID, cost_diff, p->innet_seq);
  }
  p->groupopt.cost_updated |= curr_costupd;

  if (source_index >= 0)
  {
    source_node = querysource_buffer[source_index].id;
    curr_coordupd = (p->groupopt.localcoordinator_index < 0 ||
      source_node < querysource_buffer[p->groupopt.localcoordinator_index].id);
    if (curr_coordupd)
    {
      DBGOUT("Coordinator updated to %d", source_node);
      p->groupopt.localcoordinator_index = source_index;
    }
    p->groupopt.coordinator_updated |= curr_coordupd;
  }  
}

void groupopt_coordcost(uint8_t query_id)
{
  COORDCOST_MSGHI msg_out;
  int i, size;
  uint8_t *buffer;
  uint8_t path_len;
  ADDRESS path[MAX_PATHLEN], localcoord_node;
  QUERY_ENTRY *p;

DO_STACKTEST

  p = &query_buffer[query_id];
  if (p->used && p->spec.params[PARAM_GROUPOPT] != GROUPOPT_NONE && (p->groupopt.cost_updated ||
    p->groupopt.coordinator_updated) && p->groupopt.localcoordinator_index >= 0)
  {
    localcoord_node = querysource_buffer[p->groupopt.localcoordinator_index].id;
    if (p->spec.params[PARAM_GROUPOPT] == GROUPOPT_TYPE2 || localcoord_node < TOS_NODE_ID)
    {
      msg_out.cmd = CMDHI_COORDCOST;
      msg_out.query_id = p->query_id;
      msg_out.node_count = 1;
      msg_out.nodes[0].id = TOS_NODE_ID;
      msg_out.nodes[0].cost.plancost_seq = p->innet_seq;
      msg_out.nodes[0].cost.plancost_diff = 0;
      for (i = 0; i < SRCREL_COUNT; i++)
        msg_out.nodes[0].cost.plancost_diff +=
          ((int) p->rel[i].multicast.innet_cost - p->rel[i].multicast.base_cost);

      if (p->groupopt.coordinator_updated && p->spec.params[PARAM_GROUPOPT] == GROUPOPT_TYPE2 &&
        is_singlerel(query_id, REL_T))
        for (i = 0; i < COSTBUFFER_SIZE; i++)
          if (p->groupopt.cost_buffer[i].used && p->groupopt.cost_buffer[i].node_id != TOS_NODE_ID &&
            p->groupopt.cost_buffer[i].node_id != localcoord_node &&
            p->groupopt.cost_buffer[i].source_id != localcoord_node)
          {
            DBGOUT("Adding cached cost of %d", p->groupopt.cost_buffer[i].node_id);
            msg_out.nodes[msg_out.node_count].id = p->groupopt.cost_buffer[i].node_id;
            memcpy(&msg_out.nodes[msg_out.node_count].cost, &p->groupopt.cost_buffer[i].cost,
              sizeof(COORD_COST));
            msg_out.node_count++;
          }

      DBGOUT("Sending coordinator cost message to %d", localcoord_node);
      compute_sourcepath(path, &path_len, p->groupopt.localcoordinator_index); // partial spec. path vec to source through join point
      buffer = (uint8_t *) mmalloc(marshalest_coordcostmsghi(&msg_out));
      size = marshal_coordcostmsghi(&msg_out, buffer);
      init_forward(TOS_NODE_ID, TOS_NODE_ID, localcoord_node, path_len, path,
        FORWARD_DESTRUCTIVE, size, buffer, 0, NULL, ROUTE_SUCCESS);
      mfree(buffer);

#ifdef TOSSIM
      join_logcoordcost(query_id, localcoord_node, msg_out.nodes[0].cost);
#endif
    }

    p->groupopt.cost_updated = 0; p->groupopt.coordinator_updated = 0;
  }
}

#ifdef TOSSIM
void display_costbuffer(uint8_t query_id)
{
  int i;
  COSTBUFFER_ENTRY *p;

  DBGOUT("Cost buffer:");
  DBGOUT("Node  Source  Cost diff  Cost seq  Decision  Decision seq");
  for (i = 0; i < COSTBUFFER_SIZE; i++)
  {
    p = &query_buffer[query_id].groupopt.cost_buffer[i];
    if (p->used)
      DBGOUT("%4d %7d %10d %9d %9d %13d", p->node_id, p->source_id, p->cost.plancost_diff,
        p->cost.plancost_seq, p->decision.decision, p->decision.decision_seq);
  }
}
#endif

void handle_coordcostmsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{
  COORDCOST_MSGHI msg_in, msg_out;
  int i, size;
  uint8_t *buffer, coordinator_rcv;
  uint8_t path_len;
  ADDRESS path[MAX_PATHLEN];
  GROUPOPT_ENTRY *p;

DO_STACKTEST

  DBGOUT("Coordinator cost message received from %d", source_node);
  unmarshal_coordcostmsghi(&msg_in, data);

  p = &query_buffer[msg_in.query_id].groupopt;
  coordinator_rcv = (p->localcoordinator_index >= 0) &&
    (source_node == querysource_buffer[p->localcoordinator_index].id);
  if (msg_in.node_count == 0)
  {
    DBGERR("Invalid number of entries: %d", msg_in.node_count);
    pause_sim();
    return;
  }

  DBGOUT("Received %d extra entries", msg_in.node_count-1);
  msg_out.cmd = CMDHI_COORDCOST;
  msg_out.node_count = 0;
  
  for (i = 0; i < msg_in.node_count; i++)          // update_costbuffer has to be called to update cost!
    if (update_costbuffer(msg_in.query_id, msg_in.nodes[i].id, source_node,
      msg_in.nodes[i].cost.plancost_diff, msg_in.nodes[i].cost.plancost_seq) &&
      (coordinator_rcv == 0) && msg_in.nodes[i].id != querysource_buffer[p->localcoordinator_index].id)
    {
      msg_out.nodes[msg_out.node_count].id = msg_in.nodes[i].id;
      memcpy(&msg_out.nodes[msg_out.node_count].cost, &msg_in.nodes[i].cost, sizeof(COORD_COST));
      msg_out.node_count++;
    }

#ifdef TOSSIM
  display_costbuffer(msg_in.query_id);
#endif

  if (msg_out.node_count > 0 && p->localcoordinator_index >= 0 &&
    ((query_buffer[msg_in.query_id].spec.params[PARAM_GROUPOPT] == GROUPOPT_TYPE1 &&
     querysource_buffer[p->localcoordinator_index].id < TOS_NODE_ID) ||
    (query_buffer[msg_in.query_id].spec.params[PARAM_GROUPOPT] == GROUPOPT_TYPE2 &&
      is_singlerel(msg_in.query_id, REL_T))))
  {
    DBGOUT("Relaying coordinator cost message to local coordinator %d",
      querysource_buffer[p->localcoordinator_index].id);
    compute_sourcepath(path, &path_len, p->localcoordinator_index); // partial spec. path vec to source through join point
    buffer = (uint8_t *) mmalloc(marshalest_coordcostmsghi(&msg_out));
    size = marshal_coordcostmsghi(&msg_out, buffer);
    init_forward(TOS_NODE_ID, TOS_NODE_ID, querysource_buffer[p->localcoordinator_index].id,
      path_len, path, FORWARD_DESTRUCTIVE, size, buffer, 0, NULL, ROUTE_SUCCESS);
    mfree(buffer);
  }
}

uint8_t check_costbuffer(uint8_t query_id)      // to make a decision need to have entries from all sources,
{
  int i, entry;
  GROUPOPT_ENTRY *p = &query_buffer[query_id].groupopt;

  for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)  // for nodes we join with, cost must be received, and must be received
    if (querysource_buffer[i].used)             // directly (not via coordinator)
    {
      entry = find_costbufferentry(query_id, querysource_buffer[i].id);
      if (entry < 0 || p->cost_buffer[entry].source_id != querysource_buffer[i].id)
        return 0;
    }

  if (query_buffer[query_id].spec.params[PARAM_GROUPOPT] == GROUPOPT_TYPE2)  // for bipartite queries
    for (i = 0; i < COSTBUFFER_SIZE; i++)     // cost of nodes we dont join with must be received through current target coodinator
      if (p->cost_buffer[i].used && p->cost_buffer[i].node_id != TOS_NODE_ID &&
        find_sourcenode(p->cost_buffer[i].node_id) < 0 &&
        p->cost_buffer[i].source_id != querysource_buffer[p->localcoordinator_index].id)
        return 0;

  return 1;
}

uint8_t check_decisionpending()
{
  int k;

  for (k = 0; k < QUERYBUFFER_SIZE; k++)
    if (query_buffer[k].used && (query_buffer[k].groupopt.cost_updated ||
      query_buffer[k].groupopt.coordinator_updated))
      return 1;               // dont wait on cost_received, we might not be a coordinator

  return 0;
}

void groupopt_coorddecision(uint8_t query_id)
{
  int i, diff_sum, cost_entry;
  COORDDECISION_MSGHI msg_out;
  COORDPAIRS_MSGHI msg_out1;
  int size;
  uint8_t buffer[MAX(sizeof(msg_out), sizeof(msg_out1))];
  uint8_t path_len;
  ADDRESS path[MAX_PATHLEN], coordinator_id;
  GROUPOPT_ENTRY *p;
  uint8_t old_decision;

DO_STACKTEST

  if (outputbuffer_size > 0)
    return;

  p = &query_buffer[query_id].groupopt;
  if (p->cost_received == 0 || p->localcoordinator_index < 0 ||
    ((query_buffer[query_id].spec.params[PARAM_GROUPOPT] == GROUPOPT_TYPE1 &&
    TOS_NODE_ID > querysource_buffer[p->localcoordinator_index].id) ||
    (query_buffer[query_id].spec.params[PARAM_GROUPOPT] == GROUPOPT_TYPE2 &&
    is_singlerel(query_id, REL_T)) || TOS_NODE_ID > p->globalcoordinator_node ||
    check_costbuffer(query_id) == 0))
    return;

#ifdef TOSSIM
  display_costbuffer(query_id);
#endif

  p->cost_received = 0;
  diff_sum = 0;
  for (i = 0; i < COSTBUFFER_SIZE; i++)
    if (p->cost_buffer[i].used)
      diff_sum += p->cost_buffer[i].cost.plancost_diff;

  old_decision = p->decision;
  p->decision = (diff_sum > DECISION_THRESHOLD) ? DECISION_BASE : DECISION_INNET;
  p->globalcoordinator_node = TOS_NODE_ID;
  p->sent_decisionseq++;
  DBGOUT("Coordinator taken decision %d, sequence %d", p->decision, p->sent_decisionseq);

  msg_out1.cmd = CMDHI_COORDPAIRS;
  msg_out1.query_id = query_id;
  msg_out1.remove_pairs = (p->decision == DECISION_INNET);
  msg_out1.source_count = 0;
  msg_out1.target_count = 0;
  if (query_buffer[query_id].spec.params[PARAM_GROUPOPT] == GROUPOPT_TYPE2)
  {
    for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
      if (querysource_buffer[i].used)
        msg_out1.target_nodes[msg_out1.target_count++] = querysource_buffer[i].id;

    for (i = 0; i < COSTBUFFER_SIZE; i++)
      if (p->cost_buffer[i].used && p->cost_buffer[i].node_id != TOS_NODE_ID &&
        find_sourcenode(p->cost_buffer[i].node_id) < 0)
        msg_out1.source_nodes[msg_out1.source_count++] = p->cost_buffer[i].node_id;
  } else
    for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
      if (querysource_buffer[i].used)
        msg_out1.source_nodes[msg_out1.source_count++] = querysource_buffer[i].id;

  DBGOUT("Sending coord pairs to Base");
  size = marshal_coordpairsmsghi(&msg_out1, buffer);
  init_forward(TOS_NODE_ID, TOS_NODE_ID, BASESTATION_ADDR, 0, NULL, FORWARD_DESTRUCTIVE,
    size, buffer, 0, NULL, ROUTE_SUCCESS);

  if (p->decision == DECISION_INNET && old_decision == DECISION_BASE)
    for (i = 0; i < SRCREL_COUNT; i++)
      query_buffer[query_id].rel[i].multicast.improved = 1;

  msg_out.cmd = CMDHI_COORDDECISION;
  msg_out.query_id = query_id;
  msg_out.decision.decision = p->decision;
  msg_out.decision.decision_seq = p->sent_decisionseq;
  msg_out.forward_decision = 0;

#ifdef TOSSIM
  join_logcoorddecision(query_id, TOS_NODE_ID, msg_out.decision);
#endif

  for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
    if (querysource_buffer[i].used && i != p->localcoordinator_index)
    {
      cost_entry = find_costbufferentry(query_id, querysource_buffer[i].id);
      if (cost_entry < 0)
      {
        DBGERR("Cannot find entry for %d", querysource_buffer[i].id);
        pause_sim();
        return;
      }

      if (p->cost_buffer[cost_entry].decision.decision != p->decision ||
        p->cost_buffer[cost_entry].decision.decision_seq == 0)
      {                                                     // send only unique decisions
        p->cost_buffer[cost_entry].decision.decision = p->decision;
        p->cost_buffer[cost_entry].decision.decision_seq = p->sent_decisionseq;

        DBGOUT("Sending decision to node %d", querysource_buffer[i].id);
        compute_sourcepath(path, &path_len, i);
        size = marshal_coorddecisionmsghi(&msg_out, buffer);
        init_forward(TOS_NODE_ID, TOS_NODE_ID, querysource_buffer[i].id, path_len, path,
          FORWARD_DESTRUCTIVE, size, buffer, 0, NULL, ROUTE_SUCCESS);
      } else
        DBGOUT("Skipping decision to node %d (%d, %d)", querysource_buffer[i].id,
          p->cost_buffer[cost_entry].decision.decision,
          p->cost_buffer[cost_entry].decision.decision_seq);
    }

  if (query_buffer[query_id].spec.params[PARAM_GROUPOPT] == GROUPOPT_TYPE2)
    for (i = 0; i < COSTBUFFER_SIZE; i++)
      if (p->cost_buffer[i].used && p->cost_buffer[i].node_id != TOS_NODE_ID &&
        find_sourcenode(p->cost_buffer[i].node_id) < 0)
        {                                 // check if there are nodes from which we have cost, but we dont join with
          msg_out.forward_decision = 1;   // if so, let local coordinator tell them the decision
          break;                          // do not update their costs, because later we might join with them
        }

  coordinator_id = querysource_buffer[p->localcoordinator_index].id;
  cost_entry = find_costbufferentry(query_id, coordinator_id);
  if (cost_entry < 0)
  {
    DBGERR("Cannot find entry for coordinator %d", coordinator_id);
    pause_sim();
    return;
  }

  if (msg_out.forward_decision || p->cost_buffer[cost_entry].decision.decision_seq == 0 ||
    p->cost_buffer[cost_entry].decision.decision != p->decision)
  {                                                     // send only unique decisions
    p->cost_buffer[cost_entry].decision.decision = p->decision;
    p->cost_buffer[cost_entry].decision.decision_seq = p->sent_decisionseq;

    DBGOUT("Sending decision to local coordinator %d", coordinator_id);
    compute_sourcepath(path, &path_len, p->localcoordinator_index);
    size = marshal_coorddecisionmsghi(&msg_out, buffer);
    init_forward(TOS_NODE_ID, TOS_NODE_ID, coordinator_id, path_len, path, FORWARD_DESTRUCTIVE,
      size, buffer, 0, NULL, ROUTE_SUCCESS);
  }
}

void handle_coorddecisionmsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{
  COORDDECISION_MSGHI msg_in, msg_out;
  int i, size;
  uint8_t buffer[sizeof(msg_out)];
  uint8_t path_len;
  ADDRESS path[MAX_PATHLEN];
  GROUPOPT_ENTRY *p;

DO_STACKTEST

  unmarshal_coorddecisionmsghi(&msg_in, data);
  DBGOUT("Coordinator decision message received from %d", source_node);
  DBGOUT("Decision: %d, Sequence: %d, Forward: %d", msg_in.decision.decision,
    msg_in.decision.decision_seq, msg_in.forward_decision);

  p = &query_buffer[msg_in.query_id].groupopt;
  if (source_node < p->globalcoordinator_node || (source_node == p->globalcoordinator_node &&
    msg_in.decision.decision_seq > p->rcv_decisionseq))
  {
    DBGOUT("Accepted decision");
    if (msg_in.decision.decision == DECISION_BASE && p->decision == DECISION_INNET)
      for (i = 0; i < SRCREL_COUNT; i++)
        query_buffer[msg_in.query_id].rel[i].multicast.improved = 1;

    p->globalcoordinator_node = source_node;
    p->rcv_decisionseq = msg_in.decision.decision_seq;
    p->decision = msg_in.decision.decision;
#ifdef TOSSIM
    join_logcoorddecision(msg_in.query_id, p->globalcoordinator_node, msg_in.decision);
#endif

    if (msg_in.forward_decision)
    {
      msg_out.cmd = CMDHI_COORDDECISION;
      memcpy(&msg_out.decision, &msg_in.decision, sizeof(msg_in.decision));
      msg_out.forward_decision = 0;
      for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
        if (querysource_buffer[i].used && querysource_buffer[i].id != source_node)
        {
          DBGOUT("Relaying decision to node %d", querysource_buffer[i].id);
          compute_sourcepath(path, &path_len, i);
          size = marshal_coorddecisionmsghi(&msg_out, buffer);
          init_forward(source_node, TOS_NODE_ID, querysource_buffer[i].id, path_len, path,
            FORWARD_DESTRUCTIVE, size, buffer, 0, NULL, ROUTE_SUCCESS);
        }
    }
  }
}

void handle_coordpairsmsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{
  int i, j;
  COORDPAIRS_MSGHI msg_in;

DO_STACKTEST
  
  DBGOUT("Coord pairs message received from %d", source_node);
  unmarshal_coordpairsmsghi(&msg_in, data);

  msg_in.source_nodes[msg_in.source_count++] = source_node;
#ifdef TOSSIM
  print_path(msg_in.source_nodes, msg_in.source_count, "Source nodes");
  print_path(msg_in.target_nodes, msg_in.target_count, "Target nodes");
#endif

  if (msg_in.target_count == 0)
    for (i = 0; i < msg_in.source_count-1; i++)
      for (j = i+1; j < msg_in.source_count; j++)
        if (msg_in.remove_pairs)
          remove_joinpair(msg_in.query_id, msg_in.source_nodes[i], msg_in.source_nodes[j], PAIROPT_BASE, 0);
        else add_joinpair(msg_in.query_id, msg_in.source_nodes[i], 0, NULL, msg_in.source_nodes[j],
          0, NULL, PAIROPT_BASE);
  else
    for (i = 0; i < msg_in.source_count; i++)
      for (j = 0; j < msg_in.target_count; j++)
        if (msg_in.remove_pairs)
          remove_joinpair(msg_in.query_id, msg_in.source_nodes[i], msg_in.target_nodes[j], PAIROPT_BASE, 0);
        else add_joinpair(msg_in.query_id, msg_in.source_nodes[i], 0, NULL, msg_in.target_nodes[j],
          0, NULL, PAIROPT_BASE);
}


